//
// Created by de on 3/24/21.
//
#include <State/monitor/MonitorClient.h>
#include <future>
#include <SqlController/dao/SqlSqliteDao.h>
#include "State/Basic/PlaformStateAbstract.h"
#include "TStateController.h"
#include "thread"
#include "log/SqdLoger.h"
using  namespace HsState;

TStateController::TStateController(ros::NodeHandle * p_node) {
    initHandler(p_node);
}

void TStateController::initHandler(ros::NodeHandle *p_node){

    initParam();
    int argc;


    p_stateSelection = new StateSelection(p_node);

    p_InitState  = std::shared_ptr<PlaformStateAbstract>(StateFactory::create(StateStatus::InitState, p_stateSelection) );
    p_PrepareState  = std::shared_ptr<PlaformStateAbstract>(StateFactory::create(StateStatus::PrepareState, p_stateSelection) );
    p_RunningState  = std::shared_ptr<PlaformStateAbstract>(StateFactory::create(StateStatus::RunningState, p_stateSelection) );
    p_ErrorState  = std::shared_ptr<PlaformStateAbstract>(StateFactory::create(StateStatus::ErrorState, p_stateSelection) );

    p_CurrentState = p_InitState;

    p_MonitorInterface = std::make_shared<MonitorClient>(p_stateSelection);

    p_EGloalSemaphore = std::make_shared<CSemaphore>();
    p_RunGloalSemaphore = std::make_shared<CSemaphore>();
    p_ResultGloalSemaphore = std::make_shared<CSemaphore>();

    //
    p_SqlDaoInterface = std::make_shared<SqlSqliteDao>();

    using namespace HsLog;
    SqlLoger::initHsLog();

}

TStateController::~TStateController() {
    delete p_stateSelection;
//    delete p_node;
}

bool TStateController::initDataBase() {
    return  p_SqlDaoInterface->connect() && p_SqlDaoInterface->checkTableExist();
}


void TStateController::setCurrentState(std::shared_ptr<PlaformStateAbstract> &p_RunningState) {
    if(b_StopEnable)
        throw PlaformTException(__LINE__,__FILE__, " b_StopEnable ");

    StateStatus lastStatus = p_CurrentState->getCurrentState();
    p_CurrentState = p_RunningState;
    spdlog::info( "Now state: "+getStateInfo(lastStatus)+" --> "+getStateInfo(p_CurrentState->getCurrentState()));
}

void TStateController::initParam() {
    b_StopEnable = false;
    b_CathError = false;
    ResetCount = 0;
    m_ThreadCount = 1;

    b_ReSumeAction = true;
}




void TStateController::initDevice() {

    try{
        p_CurrentState->init();
        initDataBase();
        spdlog::info("<------------ initDevice -------------->");

    }catch (PlaformTException &e){
        spdlog::warn(e.what());
        return ;
    }catch (std::exception &e)
    {
        spdlog::error(e.what());
        return ;
    }

}



// prepare -> running  -> prepare
//prepare -> running -> error -> reset -> count ++ -> prepare1
// one thread running

void TStateController::RunSignalAction() {
    if(!b_ReSumeAction)
    {
        spdlog::warn(" 停止标志 位置在 ");
    }
    p_RunGloalSemaphore->signal();
}

void TStateController::runLoop() {

//    m_ThreadCount = 1;
    b_ReSumeAction = true;
    first_enter = false;
    auto t_LastStamp= std::chrono::system_clock().now();

    while(!b_StopEnable)
    {
        spdlog::info(getpid() + "The runLoop thread is waiting ");
        p_RunGloalSemaphore->wait();
//        spdlog::info("The runLoop thread is signal ");


        try{

            setCurrentState(p_PrepareState);

            //可复位异常复位不成功，轮询三次
            for (int i = 0; i < 3; ++i) {

                p_CurrentState->reset();
                E_State state =p_CurrentState->wait();

                if(i == 2 && state == Error){
                    throw PlaformTException(__LINE__,__FILE__, " 无法复位异常");
                }
                if(state == Error)
                    continue;
                else
                    break;
            }
            if(!first_enter){
                t_LastStamp = std::chrono::system_clock().now();
                first_enter = true;
            }
            else
                {
                auto t_now = std::chrono::system_clock().now();
                //Error
//                t_now - t_LastStamp;
                double duration_voision = std::chrono::duration_cast<std::chrono::milliseconds>(t_now-t_LastStamp).count()/1000.0;
                t_LastStamp = t_now;
                spdlog::info("总耗时 : "+std::to_string(duration_voision));
            }
//            p_CurrentState->reset();
//            p_CurrentState->wait();

            setCurrentState(p_RunningState);

            p_CurrentState->start();
            --m_ThreadCount;

        }catch (PlaformTException &e){

            // throw error exception
            spdlog::warn(e.what());

            b_CathError = true;
        }catch (std::exception& e)
        {
            // throw error exception
            spdlog::warn(e.what());
            b_CathError = true;
        }

        if(b_CathError){
//            p_EGloalSemaphore->signal();
            break;
        }


        //once running
        p_ResultGloalSemaphore->signal();
    }
    p_EGloalSemaphore->signal();
    spdlog::error("The runLoop thread is finishing ");

}

// own thread running auto reset
void TStateController::monitorDeviceThread() {
    DeviceState2Commu deviceState2Commu;
    while(!b_StopEnable) {
        try{
            monitorDevice(deviceState2Commu);
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }catch (PlaformTException &e)
        {
            spdlog::warn(e.what());
//            b_StopEnable = true;
        }
    }
}

void TStateController::monitorDevice(DeviceState2Commu &deviceState2Commu) {
    p_MonitorInterface->checkDevice(deviceState2Commu);
}

WorkResult TStateController::getWorkResult() {
    p_ResultGloalSemaphore->wait();


    std::shared_ptr<WorkResult> p_result = p_stateSelection->getWorkResult();
    //spl controller dao
//    p_SqlDaoInterface->insertObject(*p_result);
    ///
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
//    p_RunGloalSemaphore->signal();

    spdlog::info("<-------- Loop once -------->");

    return *p_result;
}

void TStateController::AutoResetDevice() {

    while(true){
//        spdlog::info("The motionort thread is waiting ");

        p_EGloalSemaphore->wait();

        if(b_StopEnable)
            break ;

        // loop three time  , it  will be ready to reset the device ok
        for(int i = 0; b_CathError == true&& i< 3; i++){
            //reset device
            resetDevice();
        }

        if(b_CathError || (ResetCount > 2)){
            b_StopEnable = true;
            break;
        }else{
            ResetCount++;
        }
        p_RunGloalSemaphore->signal();

    }
    p_RunGloalSemaphore->signal();

//    spdlog::error("The motionort thread is finishing ");
}

void TStateController::resetDevice() {

    try {
        setCurrentState(p_ErrorState);
        p_CurrentState->reset();
    }catch (PlaformTException &e){

//        spdlog::warn(e.what());
        b_CathError = true;

//        spdlog::info("resetDevice: "+estate);
        setCurrentState(p_PrepareState);
    }

    b_CathError = false;
}


void TStateController::stopDevice() {
    // wait run action
    b_ReSumeAction = false;

}

void TStateController::EmpStopDevice() {
    try {

        setCurrentState(p_ErrorState);
        b_StopEnable = true;

        p_CurrentState->stop();
    }catch (PlaformTException &e){
        spdlog::warn(e.what());
    }
}



StateStatus TStateController::getDeviceStatus() {
    return p_CurrentState->getCurrentState();
}


std::string TStateController::getStateInfo(StateStatus state) {
    std::string StateName;
    switch(state)
    {
        case InitState: StateName = "InitState"; break;
        case PrepareState: StateName = "PrepareState"; break;
        case RunningState: StateName = "RunningState"; break;
        case ErrorState: StateName = "ErrorState"; break;
        case StopState: StateName = "StopState"; break;
        default:
            StateName = "invailded";
            break;
    }
    return StateName;

}

void TStateController::runningThreading() {
//    std::future<void> f2 = std::async(std::launch::async, &TStateController::AutoResetDevice, this);
//    std::this_thread::sleep_for(std::chrono::seconds(1));
//    std::future<void> f1 = std::async(std::launch::async, &TStateController::runLoop, this);
//    std::future<void> f3 = std::async(std::launch::async, &TStateController::monitorDevice, this);


    std::thread th0( &TStateController::AutoResetDevice, this);
    th0.detach();
    std::this_thread::sleep_for(std::chrono::seconds(1));


    std::thread th1( &TStateController::runLoop, this);
//    std::thread th2( &TStateController::monitorDevice, this);

    th1.detach();
//    th2.detach();
    return ;

}

void TStateController::resetDataRecord() {
    p_SqlDaoInterface->remove("all","-");
}

void TStateController::selectAllItem(std::vector<WorkResult> &v_workresult) {
    v_workresult = p_SqlDaoInterface->getAll();
}

void TStateController::resetSystemAndClearWarm() {
    try {
//        p_CurrentState.lock()
    }catch (PlaformTException &e){
        spdlog::warn(e.what());
    }
}

void TStateController::setThreadLoopCount(int count) {
    m_ThreadCount = count;
}








